1. Field of the Invention
The present invention relates to a mapping and localization method. More particularly, the present invention relates to a mapping and localization method combining with range data and image feature points.
2. Description of Related Art
A localization and navigation technique is used to estimate poses of a mobile platform moving in an actual environment, so as to achieve an accurate and stable navigation of the mobile platform. Such technique includes two main types, one is to establish an environmental digital map, and such map is taken as reference information for localizing the mobile platform, another is to simultaneously perform localization and construct the digital map during a motion process of the mobile platform (i.e., a simultaneous localization and mapping (SLAM) technique) without constructing the digital map in advance.
A technique for constructing the digital map is to first collect environmental obstacle information through a general range device deployed on the mobile platform, so as to obtain positions of obstacles in the space relative to the mobile platform, for example, coordinates information shown in FIG. 1A. Then, by collecting the spatial position information of the obstacles, the environmental digital map is gradually constructed as that shown in FIG. 1B, in which the coordinates information is converted into depth (profile) information. After the environmental digital map is constructed, the mobile platform collects the environmental information through the general range device during the motion process, and further compares the environmental information to the map for estimating its pose. As shown in FIG. 2, A represents a current pose estimate of the mobile platform, when dot line parts are overlapped to circle parts, it represents that the pose estimate of the mobile platform is in accordance with a current environmental state, and when the dot line parts and the circle parts are greatly deviated, it represents that the mobile platform cannot confirm its current correct pose.
According to the SLAM technique, mapping of the environment is not performed in advance, and during the motion process of the mobile platform, environmental features or landmarks are detected through a general range device or a visual device to serve as the reference information of the map, so as to achieve localization. During the motion process of the mobile platform, recognizable features in the environment are detected, and degree of reliability of the detected feature or landmark position is represented by an uncertainty distribution. Such technique achieves the localization function by continually detecting the environmental features and the corresponding positions in the space.
According to the above conventional localization technique of the mobile platform, regarding the first type of technique, accurate localization can be achieved only through the same type of range sensing device used for constructing the digital map. When the digital map is constructed through a high-accuracy and expensive general range device (for example, a commonly used laser range finder), the mobile platforms having the accurate localization and navigation demand in the environment are all required to equip such type of the general range device, and therefore a hardware cost of the mobile platform is greatly increased, and a type of the applicable mobile platform is limited. Therefore, whether the constructed environmental digital map can be provided to different kinds of mobile platforms (for example, smart phones, personal navigation devices, etc.) equipped with different sensing devices to implement the localization and navigation function is a key point of whether such technique can be widely applied. Moreover, the SLAM technique implemented through the general range device and the visual device can achieve the localization function only through environmental features detection. However, in an actual application, main problems are that a complicated visual device motion model has to be constructed, and excessive detection uncertainty of the environmental feature positions is liable to be occurred due to rotation, which can influence a stability and accuracy of the localization. Meanwhile, since correspondences between the environmental features cannot be established, the mobile platform cannot determine whether a region between each two environmental features is a passable region during the motion process, so that such technique cannot achieve a navigation function for the mobile platform moving in an actual environment.
European patent No. EP1176393 discloses a device for unmanned automatic underground mining, in which range data from a laser range finder are first collected to establish three-dimensional (3D) map data. Then, an inertial navigation system (INS) and vision sensor are used to localize the device. A principle thereof is that the INS is first used to estimate the pose of the device, and then the 3D map data around the pose is compared to feature points scanned by vision sensor, so as to correct the error of pose estimate. According to such patent, when the map is constructed, only range data from laser range finder are used, and an association between the range data and the image data are not established. Moreover, during the localization, the INS is required to estimate pose of the device.
P. Moghadam et al. (non-patent document 1) and Murarka et al. (non-patent document 2) provide a technique of constructing the environmental map by combining a laser range finder and stereo vision. According to such technique, 3D spatial depth information calculated based on the 3D image data are projected to a 2D plane, and a 2D map generated by projecting 3D images is combined with a 2D map constructed by the laser range finder to complete establishing the environmental map. According to such technique, two kinds of maps are combined, though a corresponding relationship thereof is not established, so that when the localization is performed through an individual sensor, a comparison error between captured data and the constructed map is great, which influences localization accuracy.
Non-patent document 1: “Improving Path Planning and Mapping Based on Stereo Vision and Lidar”, 10th International Conference on Control, Automation, Robotics and Vision, 2008 (ICARCV 2008). 17-20 Dec. 2008 Page(s):384-389.
Non-patent document 2: “Building Local Safety Maps for a Wheelchair Robot using Vision and Lasers”, Proceedings of the 3rd Canadian Conference on Computer and Robot Vision (CRV'06). 7-9 Jun. 2006 Page(s):25-25. Digital Object Identifier 10.1109/CRV.2006.20.